﻿using UnityEngine;
using System;
using System.Collections;
using Kinect2;
using Kinect2.Face;

public class HDFaceBasic : MonoBehaviour {

    KinectSensor kinectSensor;
    BodyFrameReader bodyReader;
    HighDefinitionFaceFrameSource highDefinitionFaceFrameSource;
    HighDefinitionFaceFrameReader highDefinitionFaceFrameReader;
    FaceAlignment currentFaceAlignment;
    FaceModel currentFaceModel;
    FaceModelBuilder faceModelBuilder;
    Body currentTrackedBody;
    ulong currentTrackingId;
    public Mesh faceMesh;
    GameObject status;
	GameObject face;
	string currentBuilderStatus = "Ready To Start Capture";
	int modelScale = 20;
	
	void Start ()
    {
        kinectSensor = KinectSensor.GetDefault();
        bodyReader = kinectSensor.BodyFrameSource.OpenReader();
        bodyReader.FrameArrived += BodyReader_FrameArrived;


        highDefinitionFaceFrameSource = new HighDefinitionFaceFrameSource(kinectSensor);
        highDefinitionFaceFrameSource.TrackingIdLost += HdFaceSource_TrackingIdLost;

        highDefinitionFaceFrameReader = highDefinitionFaceFrameSource.OpenReader();
        highDefinitionFaceFrameReader.FrameArrived += HdFaceReader_FrameArrived;

        currentFaceModel = new FaceModel();
        currentFaceAlignment = new FaceAlignment();

        kinectSensor.Open();

		InitializeComponent ();

		InitializeMesh();
		UpdateMesh();

	}
	public Material aMaterial;
	void Update () 
    {
        EventRouter.Check();
	}

    void OnGUI()
    {
        if (GUI.Button(new Rect((Screen.width - 100) / 2, 0, 100, 20), "Start Capture"))
            StartCapture();
    }

    void OnApplicationQuit()
    {
        if (bodyReader != null)
            bodyReader.Dispose();
        bodyReader = null;

        if (kinectSensor != null)
            kinectSensor.Close();
        kinectSensor = null;
    }

    void InitializeComponent()
    {
        status = GameObject.Find("status");
		face = GameObject.Find ("face");
		faceMesh = new Mesh();
		MeshFilter[] mf = face.GetComponents<MeshFilter> ();
		mf[0].mesh = faceMesh;
		MakeStatusText ();
    }

    void BodyReader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
    {
        CheckOnBuilderStatus();

        var frameReference = e.FrameReference;
        using (var frame = frameReference.AcquireFrame())
        {
            if (frame == null)
                return;

            if (currentTrackedBody != null)
            {
                currentTrackedBody = FindBodyWithTrackingId(frame, CurrentTrackingId);

                if (currentTrackedBody != null)
                    return;
            }

            Body selectedBody = FindClosestBody(frame);
            if (selectedBody == null)
                return;

            currentTrackedBody = selectedBody;
            CurrentTrackingId = selectedBody.TrackingId;
            highDefinitionFaceFrameSource.TrackingId = CurrentTrackingId;
        }
    }

    void HdFaceSource_TrackingIdLost(object sender, TrackingIdLostEventArgs e)
    {
        var lostTrackingID = e.TrackingId;

        if (CurrentTrackingId == lostTrackingID)
        {
            //CurrentTrackingId = 0;
            currentTrackedBody = null;
            if (faceModelBuilder != null)
            {
                faceModelBuilder.Dispose();
                faceModelBuilder = null;
            }

            highDefinitionFaceFrameSource.TrackingId = 0;
        }
    }

    void HdFaceReader_FrameArrived(object sender, HighDefinitionFaceFrameArrivedEventArgs e)
    {
        using (var frame = e.FrameReference.AcquireFrame())
        {
            if (frame == null || !frame.IsFaceTracked)
                return;

            frame.GetAndRefreshFaceAlignmentResult(currentFaceAlignment);
            UpdateMesh();
        }
    }

    void StartCapture()
    {
        StopCapture();

        faceModelBuilder = null;
        faceModelBuilder = highDefinitionFaceFrameSource.OpenModelBuilder(FaceModelBuilderAttributes.None);
        faceModelBuilder.BeginFaceDataCollection();
        faceModelBuilder.CollectionCompleted += HdFaceBuilder_CollectionCompleted;
    }

    void StopCapture()
    {
        if (faceModelBuilder != null)
        {
            faceModelBuilder.Dispose();
            faceModelBuilder = null;
        }
    }

    void HdFaceBuilder_CollectionCompleted(object sender, FaceModelBuilderCollectionCompletedEventArgs e)
    {
        var modelData = e.ModelData;
        currentFaceModel = modelData.ProduceFaceModel();
        faceModelBuilder.Dispose();
        faceModelBuilder = null;
        CurrentBuilderStatus = "Capture Complete";
    }

    Body FindClosestBody(BodyFrame bodyFrame)
    {
        Body result = null;
        double closestBodyDistance = double.MaxValue;

        Body[] bodies = new Body[bodyFrame.BodyCount];
        bodyFrame.GetAndRefreshBodyData(bodies);

        foreach (var body in bodies)
        {
            if (body.IsTracked)
            {
                var currentLocation = body.Joints[JointType.SpineBase].Position;

                var currentDistance = VectorLength(currentLocation);

                if (result == null || currentDistance < closestBodyDistance)
                {
                    result = body;
                    closestBodyDistance = currentDistance;
                }
            }
        }

        return result;
    }

    Body FindBodyWithTrackingId(BodyFrame bodyFrame, ulong trackingId)
    {
        Body result = null;

        Body[] bodies = new Body[bodyFrame.BodyCount];
        bodyFrame.GetAndRefreshBodyData(bodies);

        foreach (var body in bodies)
        {
            if (body.IsTracked)
            {
                if (body.TrackingId == trackingId)
                {
                    result = body;
                    break;
                }
            }
        }

        return result;
    }

    double VectorLength(CameraSpacePoint point)
    {
        var result = Math.Pow(point.X, 2) + Math.Pow(point.Y, 2) + Math.Pow(point.Z, 2);
        result = Math.Sqrt(result);
        return result;
    }

    void CheckOnBuilderStatus()
    {
        if (this.faceModelBuilder == null)
            return;

        string newStatus =  faceModelBuilder.CaptureStatus.ToString();
        var collectionStatus = faceModelBuilder.CollectionStatus;
        newStatus += ", " + GetCollectionStatusText(collectionStatus);
        CurrentBuilderStatus = newStatus;
    }

    string GetCollectionStatusText(FaceModelBuilderCollectionStatus status)
    {
        if ((status & FaceModelBuilderCollectionStatus.FrontViewFramesNeeded) != 0)
            return "FrontViewFramesNeeded";

        if ((status & FaceModelBuilderCollectionStatus.LeftViewsNeeded) != 0)
            return "LeftViewsNeeded";

        if ((status & FaceModelBuilderCollectionStatus.RightViewsNeeded) != 0)
            return "RightViewsNeeded";

        if ((status & FaceModelBuilderCollectionStatus.TiltedUpViewsNeeded) != 0)
            return "TiltedUpViewsNeeded";

        if ((status & FaceModelBuilderCollectionStatus.MoreFramesNeeded) != 0)
			return "MoreFramesNeeded";

		if (status == FaceModelBuilderCollectionStatus.Complete)
			return "Complete";

        return "";
    }
    ulong CurrentTrackingId
    {
        get { return currentTrackingId; }
        set { currentTrackingId = value; MakeStatusText();}
    }
    string CurrentBuilderStatus
    {
        get { return currentBuilderStatus; }
        set { currentBuilderStatus = value; MakeStatusText(); }
    }

    void MakeStatusText()
    {
        status.guiText.text = string.Format("Builder Status: {0}, Current Tracking ID: {1}", CurrentBuilderStatus, CurrentTrackingId);
    }

    void InitializeMesh()
    {


        var vertices = currentFaceModel.CalculateVerticesForAlignment(currentFaceAlignment);

        var vertices2 = new Vector3[(int)currentFaceModel.VertexCount];
        for (int i = 0; i < vertices2.Length; i++)
			vertices2[i] = new Vector3(vertices[i].X * modelScale, vertices[i].Y * modelScale, vertices[i].Z * modelScale);
		faceMesh.vertices = vertices2;
        
        var triangleIndices = currentFaceModel.TriangleIndices;
		var triangles = new int[(int)currentFaceModel.TriangleCount * 3];
        for (int i = 0; i < triangles.Length; i++)
            triangles[i] = (int)triangleIndices[i];
		faceMesh.triangles = triangles;
		faceMesh.RecalculateNormals();

		var uv = new Vector2[vertices2.Length];
		for (int i = 0; i < uv.Length; i++)
			uv [i] = Vector2.zero;
		faceMesh.uv = uv;

    }

    void UpdateMesh()
    {
        var vertices = currentFaceModel.CalculateVerticesForAlignment(currentFaceAlignment);
		var vertices2 = new Vector3[vertices.Count];
        for (int i = 0; i < vertices.Count; i++)
			vertices2[i] = new Vector3(vertices[i].X * modelScale, vertices[i].Y * modelScale, vertices[i].Z * modelScale);
		faceMesh.vertices = vertices2;
		face.GetComponent<MeshFilter> ().mesh = faceMesh;
    }

}
